#include <vector>
#include <float.h>

#pragma once

#include "common.hpp"

#define getx(x) ((x >> 16) & 0xFFFF)
#define gety(x) (x & 0xFFFF)

#define setx(x, y) (x = ((x|0xFFFF0000) & ((y << 16) | 0x0000FFFF)))
#define sety(x, y) (x = ((x|0x0000FFFF) & (y | 0xFFFF0000)))

typedef struct cost
{
    unsigned int m;
    float c;
} cost;

typedef struct li
{
    struct li* parent;
    unsigned int m;
} li;


#define left(x) ((x<<1) + 1)
#define right(x) ((x<<1) + 2)
#define parent(x) ((x-1) >> 1)

static void remove_min(cost* h, int* start, int* end, cost* r)
{
    *r = h[*start];
    (*start)++;
}

///priority queue through insertion sort, about 33% faster than heap
static void insert_sort(cost* h, int start, int end, const cost& p)
{
    int i = 0;

    for(i=end; i>=start; i--)
    {
        if(p.c >= h[i].c)
            break;
    }

    i++;

    int n;

    for(n=end+1; n>i; n--)
    {
        h[n] = h[n-1];
    }

    h[i] = p;
}

static void add_to_openset(cost* h, int* start, int* end, const cost& p)
{
    insert_sort(h, *start, *end, p);
    ++(*end);
}

static void get_path(std::vector<vec2>& p, li* root, int* num)
{
    if(root->parent!=NULL)
    {
        *num = *num + 1;
        get_path(p, root->parent, num);
    }

    if(root->parent == NULL)
        return;

    p.push_back({getx(root->m), gety(root->m)});
}

static float guess_heuristic(int sx, int sy, int ex, int ey)
{
    return 0;
    int xd = ex - sx;
    int yd = ey - sy;
    return sqrtf(xd*xd + yd*yd);
}

static std::vector<vec2> pathfind_2(int sx, int sy, int ex, int ey, pathable** map, peon& p)
{
    if(sx == ex && sy == ey)
    {
        return {{ex, ey}};
    }

    if(sx < 0)
        sx = 0;
    if(sx >= en::w)
        sx = en::w - 1;
    if(sy < 0)
        sy = 0;
    if(sy >= en::h)
        sy = en::h - 1;


    if(ex < 0)
        ex = 0;
    if(ex >= en::w)
        ex = en::w - 1;
    if(ey < 0)
        ey = 0;
    if(ey >= en::h)
        ey = en::h - 1;


    li *parent_map = (li*)calloc(en::w*en::h, sizeof(li));
    cost *openset = (cost*)calloc(en::w*en::h, sizeof(cost));
    char* done = (char*)calloc(en::w*en::h, sizeof(char));

    unsigned int pm=0;
    setx(pm, sx);
    sety(pm, sy);

    int opensetend=0, opensetstart = 0;
    openset[0].c = FLT_MAX;
    add_to_openset(openset, &opensetstart, &opensetend, (cost)
    {
        pm, guess_heuristic(sx, sy, ex, ey)
    });

    float *gs = (float*)calloc(en::w*en::h, sizeof(float));
    float *fs = (float*)calloc(en::w*en::h, sizeof(float));

    fs[0] = guess_heuristic(sx, sy, ex, ey);

    while(1)
    {
        ///no solution found
        if(opensetstart>opensetend)
        {
            break;
        }

        cost current;
        remove_min(openset, &opensetstart, &opensetend, &current);

        unsigned short cx = getx(current.m);
        unsigned short cy = gety(current.m);

        if(cx == ex && cy == ey)
        {
            int path_length = 0;

            std::vector<vec2> v;
            get_path(v, &parent_map[cx*en::h + cy], &path_length);

            free(parent_map);
            free(openset);
            free(done);
            free(gs);
            free(fs);


            return v;
        }

        done[cx*en::h + cy] = 1;

        ///iterate through neighbours
        int i, j;

        for(i=-1; i<2; i++)
        {
            if(cx + i < 0 || cx + i >= en::w)
            {
                continue;
            }
            for(j=-1; j<2; j++)
            {
                if(cy + j < 0 || cy + j >= en::h)
                {
                    continue;
                }

                if(i==0 && j==0)
                    continue;

                int mx = cx + i;
                int my = cy + j;

                ///its not in the openset, nor is it inaccessible

                if(done[mx*en::h + my]!=1 && map[mx][my] == PATHABLE)
                {
                    float tgs = gs[cx*en::h + cy] + sqrtf(i*i + j*j); ///distance between any neighbour is 1

                    ///packing two shorts into an integer saves quite a large amount of time due to reduced memory accesses
                    unsigned int b = 0;
                    setx(b, mx);
                    sety(b, my);

                    gs[mx*en::h + my] = tgs;
                    fs[mx*en::h + my] = tgs + guess_heuristic(mx, my, ex, ey);

                    ///reduced memory map goes for the parent map as well
                    parent_map[mx*en::h + my] = (li)
                    {
                        &parent_map[cx*en::h + cy], b
                    };

                    done[mx*en::h + my] = 1;

                    add_to_openset(openset, &opensetstart, &opensetend, (cost)
                    {
                        b, fs[mx*en::h + my]
                    });
                }
            }
        }
    }

    free(gs);
    free(openset);
    free(parent_map);
    free(done);
    free(fs);

    //return {{sx, sy}};
    return {};
}
